#include "bsp_i2c.h"

#define I2C_Handle   hi2c3
#define I2Cx_FLAG_TIMEOUT   10
#define MPU6050_ADDRESS			0x68
#define MPU6050_RA_PWR_MGMT_1		0x6B
#define MPU6050_RA_SMPLRT_DIV		0x19
#define MPU6050_RA_CONFIG			0x1A
#define MPU6050_RA_ACCEL_CONFIG	0x1C
#define MPU6050_RA_GYRO_CONFIG		0x1B
#define MPU6050_RA_WHO_AM_I 		0x75
#define MPU6050_ACC_OUT 			0x43
#define MPU6050_GYRO_OUT			0x43
#define MPU6050_RA_TEMP_OUT_H		0x41


/**
* @brief 写寄存器，这是提供给上层的接口
* @param slave_addr: 从机地址
* @param reg_addr: 寄存器地址
* @param len：写入的长度
* @param data_ptr: 指向要写入的数据
* @retval 正常为 0，不正常为非 0
*/
int Sensors_I2C_WriteRegister(unsigned char slave_addr,
							  unsigned char reg_addr,
							  unsigned short len,
							  unsigned char *data_ptr){
	HAL_StatusTypeDef status = HAL_OK;
	/* 检查 SENSOR 是否就绪进行下一次读写操作 */
	while (HAL_I2C_IsDeviceReady(&I2C_Handle, slave_addr,
		1, I2Cx_FLAG_TIMEOUT) == HAL_TIMEOUT);
	while (HAL_I2C_GetState(&I2C_Handle) != HAL_I2C_STATE_READY);

	status = HAL_I2C_Mem_Write(&I2C_Handle,slave_addr,reg_addr,
		I2C_MEMADD_SIZE_8BIT,data_ptr,len, len);

	while (HAL_I2C_GetState(&I2C_Handle) != HAL_I2C_STATE_READY) {

	}
	return status;
}
 
/**
* @brief 读寄存器，这是提供给上层的接口
* @param slave_addr: 从机地址
* @param reg_addr: 寄存器地址
* @param len：要读取的长度
* @param data_ptr: 指向要存储数据的指针
* @retval 正常为 0，不正常为非 0
*/
int Sensors_I2C_ReadRegister(unsigned char slave_addr,
							 unsigned char reg_addr,
							 unsigned short len,
							 unsigned char *data_ptr){
	HAL_StatusTypeDef status = HAL_OK;
	while (HAL_I2C_IsDeviceReady(&I2C_Handle, slave_addr,
		1, I2Cx_FLAG_TIMEOUT) == HAL_TIMEOUT);
	while (HAL_I2C_GetState(&I2C_Handle) != HAL_I2C_STATE_READY);

	status =HAL_I2C_Mem_Read(&I2C_Handle,slave_addr
		,reg_addr,I2C_MEMADD_SIZE_8BIT,data_ptr,len, len);

	while (HAL_I2C_GetState(&I2C_Handle) != HAL_I2C_STATE_READY) {

	}
	return status;
}
							 
/**
* @brief 写数据到 MPU6050 寄存器
* @param reg_add: 寄存器地址
* @param reg_data: 要写入的数据
* @retval
*/
void MPU6050_WriteReg(uint8_t reg_add,uint8_t reg_dat){
	Sensors_I2C_WriteRegister(MPU6050_ADDRESS,reg_add,1,&reg_dat);
}




/**
* @brief 从 MPU6050 寄存器读取数据
* @param reg_add: 寄存器地址
* @param Read：存储数据的缓冲区
* @param num：要读取的数据量
* @retval
*/
void MPU6050_ReadData(uint8_t reg_add,unsigned char* Read,uint8_t num){
	Sensors_I2C_ReadRegister(MPU6050_ADDRESS,reg_add,num,Read);
}





/**
* @brief 初始化 MPU6050 芯片
* @param
* @retval
*/
void MPU6050_Init(void){
	//在初始化之前要延时一段时间，若没有延时，则断电后再上电数据可能会出错
	HAL_Delay(100);
	//解除休眠状态
	MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00);
	//陀螺仪采样率
	MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x07);
	MPU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);
	//配置加速度传感器工作在 16G 模式
	MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x01);
	//陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
	MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);
	HAL_Delay(200);
}




/**
* @brief 读取 MPU6050 的 ID
* @param
* @retval 正常返回 1，异常返回 0
*/
uint8_t MPU6050ReadID(void){
	unsigned char Re = 0;
	MPU6050_ReadData(MPU6050_RA_WHO_AM_I,&Re,1); //读器件地址
	if (Re != 0x68) {
		//printf("检测不到 MPU6050 模块，请检查模块与开发板的接线,------%d\r\n",Re);
		return 0;
	} else {
		//printf("MPU6050 ID = %d\r\n",Re);
		return 1;
	}
}


/**
* @brief 读取 MPU6050 的加速度数据
* @param
* @retval
*/
void MPU6050ReadAcc(short *accData){
	uint8_t buf[6];
	MPU6050_ReadData(MPU6050_ACC_OUT, buf, 6);
	accData[0] = (buf[0] << 8) | buf[1];
	accData[1] = (buf[2] << 8) | buf[3];
	accData[2] = (buf[4] << 8) | buf[5];
}
/**
* @brief 读取 MPU6050 的角加速度数据
* @param
* @retval
*/
void MPU6050ReadGyro(short *gyroData){
	uint8_t buf[6];
	MPU6050_ReadData(MPU6050_GYRO_OUT,buf,6);
	gyroData[0] = (buf[0] << 8) | buf[1];
	gyroData[1] = (buf[2] << 8) | buf[3];
	gyroData[2] = (buf[4] << 8) | buf[5];
}
/**
* @brief 读取 MPU6050 的原始温度数据
* @param
* @retval
*/
void MPU6050ReadTemp(short *tempData){
	uint8_t buf[2];
	MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2); //读取温度值
	*tempData = (buf[0] << 8) | buf[1];
}
/**
* @brief 读取 MPU6050 的温度数据，转化成摄氏度
* @param
* @retval
*/
void MPU6050_ReturnTemp(float*Temperature){
	short temp3;
	uint8_t buf[2];
	MPU6050_ReadData(MPU6050_RA_TEMP_OUT_H,buf,2); //读取温度值
	temp3= (buf[0] << 8) | buf[1];
	*Temperature=((double) (temp3 /340.0))+36.53;
}


void I2C_DEADLOCK_Handler(){
	MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x01);
	// 释放I2C端口为GPIO
    HAL_I2C_DeInit(&I2C_Handle);

    // 强制拉高SCL和SDA
    /* IIC1 */
//	GPIO_InitTypeDef GPIO_InitStruct;
//	GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
//    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
//    GPIO_InitStruct.Pull = GPIO_NOPULL;
//    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
//    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
//    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6 | GPIO_PIN_7, GPIO_PIN_SET);
//	HAL_Delay(10);
//	HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6 | GPIO_PIN_7);


    /* IIC3 */
	GPIO_InitTypeDef GPIO_InitStruct;
	GPIO_InitStruct.Pin = GPIO_PIN_8;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_8, GPIO_PIN_SET);
	HAL_Delay(10);
	HAL_GPIO_DeInit(GPIOA, GPIO_PIN_8);
	HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8);

    // 重新初始化I2C控制器
    HAL_I2C_Init(&I2C_Handle);
}

